
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       mb_motors_multicopter.h
  * @author     baiyang
  * @date       2023-1-30
  ******************************************************************************
  */

#pragma once

#ifdef __cplusplus
extern "C"{
#endif

/*----------------------------------include-----------------------------------*/
#include "mb_motors_class.h"
/*-----------------------------------macro------------------------------------*/
#ifndef MB_MOTORS_DENSITY_COMP
#define MB_MOTORS_DENSITY_COMP 1
#endif

#define MB_MOTORS_YAW_HEADROOM_DEFAULT  200
#define MB_MOTORS_THST_EXPO_DEFAULT     0.65f   // set to 0 for linear and 1 for second order approximation
#define MB_MOTORS_THST_HOVER_DEFAULT    0.35f   // the estimated hover throttle, 0 ~ 1
#define MB_MOTORS_THST_HOVER_TC         10.0f   // time constant used to update estimated hover throttle, 0 ~ 1
#define MB_MOTORS_THST_HOVER_MIN        0.125f  // minimum possible hover throttle
#define MB_MOTORS_THST_HOVER_MAX        0.6875f // maximum possible hover throttle
#define MB_MOTORS_SPIN_MIN_DEFAULT      0.15f   // throttle out ratio which produces the minimum thrust.  (i.e. 0 ~ 1 ) of the full throttle range
#define MB_MOTORS_SPIN_MAX_DEFAULT      0.95f   // throttle out ratio which produces the maximum thrust.  (i.e. 0 ~ 1 ) of the full throttle range
#define MB_MOTORS_SPIN_ARM_DEFAULT      0.10f   // throttle out ratio which produces the armed spin rate.  (i.e. 0 ~ 1 ) of the full throttle range
#define MB_MOTORS_BAT_VOLT_MAX_DEFAULT  0.0f    // voltage limiting max default
#define MB_MOTORS_BAT_VOLT_MIN_DEFAULT  0.0f    // voltage limiting min default (voltage dropping below this level will have no effect)
#define MB_MOTORS_BAT_CURR_MAX_DEFAULT  0.0f    // current limiting max default
#define MB_MOTORS_BAT_CURR_TC_DEFAULT   5.0f    // Time constant used to limit the maximum current
#define MB_MOTORS_BATT_VOLT_FILT_HZ     0.5f    // battery voltage filtered at 0.5hz
#define MB_MOTORS_SLEW_TIME_DEFAULT     0.0f    // slew rate limit for thrust output
#define MB_MOTORS_SAFE_TIME_DEFAULT     1.0f    // Time for the esc when transitioning between zero pwm to minimum

// spool definition
#define MB_MOTORS_SPOOL_UP_TIME_DEFAULT 0.5f    // time (in seconds) for throttle to increase from zero to min throttle, and min throttle to full throttle.
/*----------------------------------typedef-----------------------------------*/
// set thrust compensation callback
typedef void (*thrust_compensation_fn_t)(float *, uint8_t);

// enum values for HOVER_LEARN parameter
enum MotorHoverLearn {
    MB_MOTOR_HOVER_LEARN_DISABLED = 0,
    MB_MOTOR_HOVER_LEARN_ONLY = 1,
    MB_MOTOR_HOVER_LEARN_AND_SAVE = 2
};

typedef struct mb_motors_multicopter* mb_motors_mc_t;
typedef const struct mb_motors_multicopter* mb_motors_mc_const_t;
typedef struct mb_motors_multicopter_ops* mb_motors_mc_ops_t;

struct mb_motors_multicopter {
    struct mb_motors motors;

    mb_motors_mc_ops_t ops_mc;

    // parameters
    Param_int16            _yaw_headroom;          // yaw control is given at least this pwm range
    Param_float            _thrust_curve_expo;     // curve used to linearize pwm to thrust conversion.  set to 0 for linear and 1 for second order approximation
    Param_float            _slew_up_time;          // throttle increase slew limitting
    Param_float            _slew_dn_time;          // throttle decrease slew limitting
    Param_float            _safe_time;             // Time for the esc when transitioning between zero pwm to minimum
    Param_float            _spin_min;              // throttle out ratio which produces the minimum thrust.  (i.e. 0 ~ 1 ) of the full throttle range
    Param_float            _spin_max;              // throttle out ratio which produces the maximum thrust.  (i.e. 0 ~ 1 ) of the full throttle range
    Param_float            _spin_arm;              // throttle out ratio which produces the armed spin rate.  (i.e. 0 ~ 1 ) of the full throttle range
    Param_float            _batt_voltage_max;      // maximum voltage used to scale lift
    Param_float            _batt_voltage_min;      // minimum voltage used to scale lift
    Param_float            _batt_current_max;      // current over which maximum throttle is limited
    Param_float            _batt_current_time_constant;    // Time constant used to limit the maximum current
    Param_int8             _batt_idx;              // battery index used for compensation
    Param_int16            _pwm_min;               // minimum PWM value that will ever be output to the motors (if 0, vehicle's throttle input channel's min pwm used)
    Param_int16            _pwm_max;               // maximum PWM value that will ever be output to the motors (if 0, vehicle's throttle input channel's max pwm used)
    Param_float            _throttle_hover;        // estimated throttle required to hover throttle in the range 0 ~ 1
    Param_int8             _throttle_hover_learn;  // enable/disabled hover thrust learning
    Param_int8             _disarm_disable_pwm;    // disable PWM output while disarmed

    // Maximum lean angle of yaw servo in degrees. This is specific to tricopter
    Param_float            _yaw_servo_angle_max_deg;

    // time to spool motors to min throttle
    Param_float            _spool_up_time;

    // scaling for booster motor throttle
    Param_float            _boost_scale;

    // motor output variables
    bool                motor_enabled[MB_MOTORS_MAX_NUM_MOTORS];    // true if motor is enabled

    // spool variables
    float               _spin_up_ratio;      // throttle percentage (0 ~ 1) between zero and throttle_min

    // battery voltage, current and air pressure compensation variables
    LowPassFiltFloat    _batt_voltage_filt;     // filtered battery voltage expressed as a percentage (0 ~ 1.0) of batt_voltage_max
    float               _lift_max;              // maximum lift ratio from battery voltage
    float               _throttle_limit;        // ratio of throttle limit between hover and maximum
    float               _throttle_thrust_max;   // the maximum allowed throttle thrust 0.0 to 1.0 in the range throttle_min to throttle_max
    float               _disarm_safe_timer;     // Timer for the esc when transitioning between zero pwm to minimum

    // vehicle supplied callback for thrust compensation. Used for tiltrotors and tiltwings
    thrust_compensation_fn_t _thrust_compensation_callback;

    // array of motor output values
    float               _actuator[MB_MOTORS_MAX_NUM_MOTORS];
};

struct mb_motors_multicopter_ops {
    // output a thrust to all motors that match a given motor
    // mask. This is used to control tiltrotor motors in forward
    // flight. Thrust is in the range 0 to 1
    void        (*output_motor_mask)(mb_motors_t motors, float thrust, uint16_t mask, float rudder_dt);

    // disable the use of motor torque to control yaw. Used when an external mechanism such
    // as vectoring is used for yaw control
    void        (*disable_yaw_torque)(mb_motors_t motors);

    // output_to_motors - sends commands to the motors
    void        (*output_to_motors)(mb_motors_t motors);

    // return current_limit as a number from 0 ~ 1 in the range throttle_min to throttle_max
    float       (*get_current_limit_max_throttle)(mb_motors_t motors);

    // apply any thrust compensation for the frame
    void        (*thrust_compensation)(mb_motors_t motors);

    // output booster throttle, if any
    void        (*output_boost_throttle)(mb_motors_t motors);

    // output roll/pitch/yaw/thrust
    void        (*output_rpyt)(mb_motors_t motors);
};

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void                mb_motors_multicopter_ctor(mb_motors_mc_t motors_multicopter, mb_motors_mc_ops_t ops_mc, mb_motors_ops_t ops, uint16_t speed_hz);

// update_throttle_range - update throttle endpoints
void                mb_motors_update_throttle_range(mb_motors_t motors);

// update the throttle input filter.  should be called at 100hz
void                mb_motors_update_throttle_hover(mb_motors_t motors, float dt);

// passes throttle directly to all motors for ESC calibration.
//   throttle_input is in the range of 0 ~ 1 where 0 will send get_pwm_output_min() and 1 will send get_pwm_output_max()
void                mb_motors_set_throttle_passthrough_for_esc_calibration(mb_motors_t motors, float throttle_input);

// parameter check for MOT_PWM_MIN/MAX, returns true if parameters are valid
bool                mb_motors_check_mot_pwm_params(mb_motors_const_t motors);

// converts desired thrust to linearized actuator output in a range of 0~1
float               mb_motors_thrust_to_actuator(mb_motors_const_t motors, float thrust_in);

// inverse of above, tested with AP_Motors/examples/expo_inverse_test
// used to calculate equivelent motor throttle level to direct ouput, used in tailsitter transtions
float               mb_motors_actuator_to_thrust(mb_motors_const_t motors, float actuator);

// apply_thrust_curve_and_volt_scaling - returns throttle in the range 0 ~ 1
float               mb_motors_apply_thrust_curve_and_volt_scaling(mb_motors_const_t motors, float thrust);

// inverse of above, tested with AP_Motors/examples/expo_inverse_test
// used to calculate equivelent motor throttle level to direct ouput, used in tailsitter transtions
float               mb_motors_remove_thrust_curve_and_volt_scaling(mb_motors_const_t motors, float throttle);

// convert values to PWM min and max if not configured
void                mb_motors_convert_pwm_min_max_param(mb_motors_t motors, int16_t radio_min, int16_t radio_max);

// run spool logic
void                mb_motors_output_logic(mb_motors_t motors);

// update_lift_max from battery voltage - used for voltage compensation
void                mb_motors_update_lift_max_from_batt_voltage(mb_motors_t motors);

// return gain scheduling gain based on voltage and air density
float               mb_motors_get_compensation_gain(mb_motors_const_t motors);

// convert actuator output (0~1) range to pwm range
int16_t             mb_motors_output_to_pwm(mb_motors_t motors, float actuator);

// adds slew rate limiting to actuator output
void                mb_motors_set_actuator_with_slew(mb_motors_t motors, float* actuator_output, float input);

// gradually increase actuator output to spin_min
float               mb_motors_actuator_spin_up_to_ground_idle(mb_motors_const_t motors);

void                mb_motors_bind_motors_function(mb_motors_ops_t ops_frontend, mb_motors_ops_t ops_multicopter);

// set_yaw_headroom - set yaw headroom (yaw is given at least this amount of pwm)
static inline void  mb_motors_set_yaw_headroom(mb_motors_t motors, int16_t pwm) { ((mb_motors_mc_t)motors)->_yaw_headroom = pwm; }

// returns maximum thrust in the range 0 to 1
static inline float mb_motors_get_throttle_thrust_max(mb_motors_const_t motors) { return ((mb_motors_mc_const_t)motors)->_throttle_thrust_max; }

// return true if spool up is complete
static inline bool  mb_motors_spool_up_complete(mb_motors_const_t motors) { return motors->_spool_state == MB_MOTOR_THROTTLE_UNLIMITED; }

// get minimum or maximum pwm value that can be output to motors
static inline int16_t mb_motors_get_pwm_output_min(mb_motors_const_t motors) { return ((mb_motors_mc_const_t)motors)->_pwm_min; }
static inline int16_t mb_motors_get_pwm_output_max(mb_motors_const_t motors) { return ((mb_motors_mc_const_t)motors)->_pwm_max; }

// set thrust compensation callback
static inline void  mb_motors_set_thrust_compensation_callback(mb_motors_t motors, thrust_compensation_fn_t callback) {
    ((mb_motors_mc_t)motors)->_thrust_compensation_callback = callback;
}


/// 
void                mb_motors_OutputMotorMask(mb_motors_t motors, float thrust, uint16_t mask, float rudder_dt);
float               mb_motors_GetCurrentLimitMaxThrottle(mb_motors_t motors);
void                mb_motors_OutputBoostThrottle(mb_motors_t motors);
void                mb_motors_OutputRpyt(mb_motors_t motors);
uint32_t            mb_motors_GetMotorMask(mb_motors_t motors);


/// ops

// output a thrust to all motors that match a given motor
// mask. This is used to control tiltrotor motors in forward
// flight. Thrust is in the range 0 to 1
static inline void  mb_motors_output_motor_mask(mb_motors_t motors, float thrust, uint16_t mask, float rudder_dt) {
    if (((mb_motors_mc_t)motors)->ops_mc->output_motor_mask) {
        ((mb_motors_mc_t)motors)->ops_mc->output_motor_mask(motors, thrust, mask, rudder_dt);
        return;
    }

    mb_motors_OutputMotorMask(motors, thrust, mask, rudder_dt);
}

// disable the use of motor torque to control yaw. Used when an external mechanism such
// as vectoring is used for yaw control
static inline void  mb_motors_disable_yaw_torque(mb_motors_t motors) {
    if (((mb_motors_mc_t)motors)->ops_mc->disable_yaw_torque) {
        ((mb_motors_mc_t)motors)->ops_mc->disable_yaw_torque(motors);
    }
}

// output_to_motors - sends commands to the motors
static inline void  mb_motors_output_to_motors(mb_motors_t motors) {
    RT_ASSERT(((mb_motors_mc_t)motors)->ops_mc->output_to_motors != RT_NULL);
    ((mb_motors_mc_t)motors)->ops_mc->output_to_motors(motors);
}

// return current_limit as a number from 0 ~ 1 in the range throttle_min to throttle_max
static inline float mb_motors_get_current_limit_max_throttle(mb_motors_t motors) {
    if (((mb_motors_mc_t)motors)->ops_mc->get_current_limit_max_throttle) {
        return ((mb_motors_mc_t)motors)->ops_mc->get_current_limit_max_throttle(motors);
    }

    return mb_motors_GetCurrentLimitMaxThrottle(motors);
}

// apply any thrust compensation for the frame
static inline void  mb_motors_thrust_compensation(mb_motors_t motors) {
    if (((mb_motors_mc_t)motors)->ops_mc->thrust_compensation) {
        ((mb_motors_mc_t)motors)->ops_mc->thrust_compensation(motors);
    }
}

// output booster throttle, if any
static inline void  mb_motors_output_boost_throttle(mb_motors_t motors) {
    if (((mb_motors_mc_t)motors)->ops_mc->output_boost_throttle) {
        ((mb_motors_mc_t)motors)->ops_mc->output_boost_throttle(motors);
        return;
    }

    mb_motors_OutputBoostThrottle(motors);
}

// output roll/pitch/yaw/thrust
static inline void  mb_motors_output_rpyt(mb_motors_t motors) {
    if (((mb_motors_mc_t)motors)->ops_mc->output_rpyt) {
        ((mb_motors_mc_t)motors)->ops_mc->output_rpyt(motors);
        return;
    }

    mb_motors_OutputRpyt(motors);
}

/*------------------------------------test------------------------------------*/

#ifdef __cplusplus
}
#endif



